"""
Author: Morphlng
Date: 2024-03-19 13:48:16
LastEditTime: 2024-03-19 13:48:24
LastEditors: Morphlng
Description: Test for vector2d
FilePath: /DrivingGym/tests/common/geom/vector2d_test.py
"""

import math

import numpy as np

from driving_gym.simulation.common.geom import Vector2D


def is_close(a, b):
    return math.isclose(a + 1, b + 1)


class TestVector2D:

    def test_initialization(self):
        vec = Vector2D(3, 4)
        assert vec.x == 3 and vec.y == 4, "Initialization or attributes failed."

    def test_as_numpy_array(self):
        vec = Vector2D(3, 4)
        assert np.array_equal(
            vec.as_numpy_array(), np.array([3, 4])
        ), "Conversion to numpy array failed."

    def test_get_angle(self):
        vec1 = Vector2D(1, 0)
        vec2 = Vector2D(0, 1)
        assert is_close(vec1.get_angle(vec2), -math.pi / 2), "Angle computation failed."

        assert is_close(vec2.get_angle(vec1), math.pi / 2), "Angle computation failed."

        assert is_close(vec1.get_angle(vec1), 0), "Angle computation failed."

    def test_l1_distance(self):
        vec1 = Vector2D(1, 2)
        vec2 = Vector2D(4, 6)
        assert vec1.l1_distance(vec2) == 7, "L1 distance computation failed."

    def test_l2_distance(self):
        vec1 = Vector2D(0, 3)
        vec2 = Vector2D(4, 0)
        assert is_close(vec1.l2_distance(vec2), 5), "L2 distance computation failed."

    def test_magnitude(self):
        vec = Vector2D(3, 4)
        assert is_close(vec.magnitude(), 5), "Magnitude computation failed."

    def test_length(self):
        vec = Vector2D(3, 4)
        assert is_close(vec.length(), 5), "Length computation failed."

    def test_squared_length(self):
        vec = Vector2D(3, 4)
        assert vec.squared_length() == 25, "Squared length computation failed."

    def test_make_unit_vector(self):
        vec = Vector2D(3, 4)
        unit_vec = vec.make_unit_vector()
        assert is_close(unit_vec.magnitude(), 1), "Unit vector computation failed."

    def test_copy(self):
        vec = Vector2D(3, 4)
        vec_copy = vec.copy()
        assert vec_copy == vec and vec_copy is not vec, "Copy method failed."

    def test_add(self):
        vec1 = Vector2D(1, 2)
        vec2 = Vector2D(3, 4)
        vec3 = vec1 + vec2
        assert vec3.x == 4 and vec3.y == 6, "Vector addition failed."

    def test_sub(self):
        vec1 = Vector2D(5, 7)
        vec2 = Vector2D(2, 3)
        vec3 = vec1 - vec2
        assert vec3.x == 3 and vec3.y == 4, "Vector subtraction failed."

    def test_mul(self):
        vec = Vector2D(3, 4)
        vec_scaled = vec * 2
        assert vec_scaled.x == 6 and vec_scaled.y == 8, "Scalar multiplication failed."

    def test_eq(self):
        vec1 = Vector2D(1, 2)
        vec2 = Vector2D(1, 2)
        assert vec1 == vec2, "Equality check failed."

    def test_ne(self):
        vec1 = Vector2D(1, 2)
        vec2 = Vector2D(2, 1)
        assert vec1 != vec2, "Inequality check failed."
